package SGEngine2

import (
	"math"
)

type Mat4 [16]float64

var (
	//	The 4x4 identity matrix.
	Mat4Identity Mat4

	m4z Mat4
)

func init() {
	Mat4Identity[0], Mat4Identity[4], Mat4Identity[8], Mat4Identity[12] = 1, 0, 0, 0
	Mat4Identity[1], Mat4Identity[5], Mat4Identity[9], Mat4Identity[13] = 0, 1, 0, 0
	Mat4Identity[2], Mat4Identity[6], Mat4Identity[10], Mat4Identity[14] = 0, 0, 1, 0
	Mat4Identity[3], Mat4Identity[7], Mat4Identity[11], Mat4Identity[15] = 0, 0, 0, 1
}

func (m1 *Mat4) Mult(m2 *Mat4) *Mat4 {
	var mat Mat4 = Mat4Identity
	mat[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3]
	mat[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3]
	mat[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3]
	mat[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3]

	mat[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7]
	mat[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7]
	mat[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7]
	mat[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7]

	mat[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11]
	mat[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11]
	mat[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11]
	mat[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11]

	mat[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15]
	mat[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15]
	mat[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15]
	mat[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15]
	return &mat
}

func (m *Mat4) Translation(vec *Vec3) {
	m[12] = vec.X
	m[13] = vec.Y
	m[14] = vec.Z
	m[15] = 1.0
}

func (m *Mat4) Position() Vec3 {
	return Vec3{m[12], m[13], m[14]}
}

func (m *Mat4) Scale() *Vec3 {
	return &Vec3{m[0], m[5], m[10]}
}

func (m *Mat4) Scaling(vec *Vec3) {
	m[0] = vec.X
	m[5] = vec.Y
	m[10] = vec.Z
	m[15] = 1.0

}

func (m *Mat4) RotationPitchYawRoll(pitch, yaw, roll float64) {
	cr := math.Cos(pitch)
	sr := math.Sin(pitch)
	cp := math.Cos(yaw)
	sp := math.Sin(yaw)
	cy := math.Cos(roll)
	sy := math.Sin(roll)
	srsp := sr * sp
	crsp := cr * sp
	m[0] = cp * cy
	m[4] = cp * sy
	m[8] = -sp
	m[1] = srsp*cy - cr*sy
	m[5] = srsp*sy + cr*cy
	m[9] = sr * cp
	m[2] = crsp*cy + sr*sy
	m[6] = crsp*sy - sr*cy
	m[10] = cr * cp
	m[3], m[7], m[11] = 0.0, 0, 0
	m[15] = 1.0
}
